/*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
*     * Redistributions of source code must retain the above copyright
*       notice, this list of conditions and the following disclaimer.
*     * Redistributions in binary form must reproduce the above copyright
*       notice, this list of conditions and the following disclaimer in the
*       documentation and/or other materials provided with the distribution.
*     * Neither the name of the Willow Garage, Inc. nor the names of its
*       contributors may be used to endorse or promote products derived from
*       this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/

/*
* This file contains helper functions for loading images as maps.
*
* Author: Brian Gerkey
*/

#include <cstring>
#include <stdexcept>

#include <stdlib.h>
#include <stdio.h>

// We use SDL_image to load the image from disk
#include <SDL/SDL_image.h>

// Use Bullet's Quaternion object to create one from Euler angles
#include <LinearMath/btQuaternion.h>

#include "map_server/image_loader.h"

// compute linear index for given map coords
#define MAP_IDX(sx, i, j) ((sx) * (j) + (i))

namespace map_server
{

/** 从文件中读取图像并填写respp对象，以便稍后在请求服务时使用。
 * @param resp     map写出的路径
 * @param fname    image读取的路径
 * @param res      map的分辨率
 * @param negate   如果是true，白色是占据栅格，黑色是空闲
 * @param occ_th   占据栅格的阈值
 * @param free_th  空闲栅格的阈值
 * @param origin   图像左下角的2D坐标，x、y、yaw，yaw是逆时针
 * @param mode     地图模式
 * @throws std::runtime_error If the image file can't be loaded
 * 该结构有三种表示方法:
     Trinary，即占据（像素用100表示），free（像素用0表示）、不明（像素用-1表示）；
     Scale，占据与free与三值一样，但是不明变成了(0,100)；
     Raw，所有像素用[0, 255]表示。
*/
void loadMapFromFile(nav_msgs::GetMap::Response *resp,
                const char *fname, double res, bool negate,
                double occ_th, double free_th, double *origin, MapMode mode)
{
    SDL_Surface *img;
    unsigned char *pixels;
    unsigned char *p;
    unsigned char value;
    int rowstride, n_channels, avg_channels;
    unsigned int i, j;
    int k;
    double occ;
    int alpha;
    int color_sum;
    double color_avg;
    // Load the image using SDL.  If we get NULL back, the image load failed.
    if (!(img = IMG_Load(fname)))
    {
        std::string errmsg = std::string("failed to open image file \"") +
                             std::string(fname) + std::string("\": ") + IMG_GetError();
        throw std::runtime_error(errmsg);
    }
    // Copy the image data into the map structure
    resp->map.info.width = img->w;
    resp->map.info.height = img->h;
    resp->map.info.resolution = res;
    resp->map.info.origin.position.x = *(origin);
    resp->map.info.origin.position.y = *(origin + 1);
    resp->map.info.origin.position.z = 0.0;
    btQuaternion q;
    // setEulerZYX(yaw, pitch, roll)
    q.setEulerZYX(*(origin + 2), 0, 0);
    resp->map.info.origin.orientation.x = q.x();
    resp->map.info.origin.orientation.y = q.y();
    resp->map.info.origin.orientation.z = q.z();
    resp->map.info.origin.orientation.w = q.w();
    // Allocate space to hold the data
    resp->map.data.resize(resp->map.info.width * resp->map.info.height);
    // Get values that we'll need to iterate through the pixels
    rowstride = img->pitch;
    n_channels = img->format->BytesPerPixel;
    // NOTE: Trinary mode still overrides here to preserve existing behavior.
    // Alpha will be averaged in with color channels when using trinary mode.
    if (mode == TRINARY || !img->format->Amask)
        avg_channels = n_channels;
    else avg_channels = n_channels - 1;
    // 将像素数据复制到地图结构中
    pixels = (unsigned char *) (img->pixels);
    for (j = 0; j < resp->map.info.height; j++)
    {
        for (i = 0; i < resp->map.info.width; i++)
        {
            // 计算该像素的RGB平均值
            p = pixels + j * rowstride + i * n_channels;
            color_sum = 0;
            for (k = 0; k < avg_channels; k++) color_sum += *(p + (k));
            color_avg = color_sum / (double) avg_channels;
            if (n_channels == 1) alpha = 1;
            else alpha = *(p + n_channels - 1);
            if (negate) color_avg = 255 - color_avg;
            if (mode == RAW)
            {
                value = color_avg;
                resp->map.data[MAP_IDX(resp->map.info.width, i, resp->map.info.height - j - 1)] = value;
                continue;
            }
            // If negate is true, we consider blacker pixels free, and whiter pixels occupied.  Otherwise, it's vice versa.
            // 如果negative为true，则认为黑色像素空闲，白色像素被占用。反之亦然。
            occ = (255 - color_avg) / 255.0;
            // Apply thresholds to RGB means to determine occupancy values for map.
            // Note that we invert the graphics-ordering of the pixels to produce a map with cell (0,0) in the lower-left corner.
            // 为RGB方法应用阈值来确定地图的占用值。
            // 注意，我们颠倒了像素的图形顺序，生成了一个单元格(0,0)位于左下角的地图。
            if (occ > occ_th) value = +100;
            else if (occ < free_th) value = 0;
            else if (mode == TRINARY || alpha < 1.0) value = -1;
            else
            {
                double ratio = (occ - free_th) / (occ_th - free_th);
                value = 1 + 98 * ratio;
            }
            resp->map.data[MAP_IDX(resp->map.info.width, i, resp->map.info.height - j - 1)] = value;
        }
    }
    SDL_FreeSurface(img);
}

}
